#ifndef MATRIX_H
#define MATRIX_H

#include <iostream>
#include <iomanip>
#include <math.h>

using namespace std;


template <class T>
class Matrix
{
public:
	Matrix();
	Matrix(unsigned int newHeight, unsigned int newWidth, const T* data = 0);
	Matrix(const Matrix& rhs);
	~Matrix();

	void Import(const T* data);
	void ImportUnsignedChars(const unsigned char* data);
	void ImportSignedInts(const signed int* data);
	void ImportSignedChars(const signed char* data);
	void ExportUnsignedChars(unsigned char* data);
	void ExportSignedInts(signed int* data);
	void ExportSignedChars(signed char* data);
	void Reshape(unsigned int newHeight, unsigned int newWidth);
	void Multiply(const Matrix& rhs);
	Matrix& Scale(T scale);
	Matrix& ScaleInv(T scale);
	void Assign(const Matrix& rhs);
	void Add(const Matrix& rhs);
	void Substract(const Matrix& rhs);
	Matrix& DivideElementsBy(const Matrix& rhs);
	Matrix& MultiplyElementsBy(const Matrix& rhs);
	Matrix& Transpose();
	Matrix Transposed();
	void Floor();
	void Round();
	void Clip(T minimum, T maximum);
	Matrix& Zigzag();
	Matrix& ZigzagInv();

	void SetElement(unsigned int y, unsigned int x, T value)
	{
		if(x >= width || y >= height)
			throw "Matrix: SetElement out of bounds";

		matrix[x + y * width] = value;
	}

	T GetElement(unsigned int y, unsigned int x) const
	{	
		if(x >= width || y >= height)
			throw "Matrix: GetElement out of bounds";

		return matrix[x + y * width]; 
	}


	Matrix operator*(const Matrix& rhs)
	{
		Matrix result = *this;

		result *= rhs;

		return result;
	}

	Matrix operator+(const Matrix& rhs)
	{
		Matrix result = *this;

		result += rhs;

		return result;
	}

	Matrix operator-(const Matrix& rhs)
	{
		Matrix result = *this;

		result -= rhs;

		return result;
	}

	Matrix& operator=(const Matrix& rhs)
	{
		Assign(rhs);

		return *this;
	}

	Matrix& operator*=(const Matrix& rhs)
	{
		Multiply(rhs);

		return *this;
	}

	Matrix& operator+=(const Matrix& rhs)
	{
		Add(rhs);

		return *this;
	}

	Matrix& operator-=(const Matrix& rhs)
	{
		Substract(rhs);

		return *this;
	}


	T& operator()(unsigned int y, unsigned int x);

	unsigned int GetWidth() const
	{
		return width;
	}

	unsigned int GetHeight() const
	{
		return height;
	}

	unsigned int GetSize() const
	{
		return width*height;
	}

protected:

private:
	void Initialize();
	void Clear();

	unsigned int width, height;

	T* matrix;

	
};


template <class T>
Matrix<T>::Matrix(): matrix(0), width(0), height(0)
{
}

template <class T>
Matrix<T>::Matrix(const Matrix& rhs): matrix(0), width(0), height(0)
{
	Assign(rhs);
}


template <class T>
Matrix<T>::Matrix(unsigned int newHeight, unsigned int newWidth, const T* data)
{
	height = newHeight;
	width = newWidth;
	
	matrix = new T[height * width];

	Initialize();
	
	if (data)
		Import(data);
}

template <class T>
void Matrix<T>::Initialize()
{
	unsigned int i, j;

	for(i = 0; i < height; i++)
		for(j = 0; j < width; j++)
		{
			matrix[j + width * i] = 0;
		}
}

template <class T>
void Matrix<T>::Clear()
{
	if (matrix)
	{
		delete[] matrix;
		matrix = 0;
	}
}

template <class T>
Matrix<T>::~Matrix()
{
	Clear();
}

template <class T>
void Matrix<T>::Reshape(unsigned int newHeight, unsigned int newWidth)
{
	unsigned int newsize = newHeight * newWidth;
	unsigned int oldsize = width * height;
	unsigned int copysize = newsize;

	if (copysize > oldsize)
		copysize = oldsize;


	T *newmatrix = new T[newsize];

	for(unsigned int i=0; i<newsize; i++)
	{
		if (i < copysize)
			newmatrix[i] = matrix[i];
		else
			newmatrix[i] = 0;
	}

	Clear();

	matrix = newmatrix;
	width = newWidth;
	height = newHeight;
}

template <class T>
Matrix<T>& Matrix<T>::Zigzag()
{
	int xbl = 0, ybl = 0, xtr = 0, ytr = 0;
	int widthmin = width-1, heightmin = height-1;
	int d = -1;
	unsigned int size = height * width;
	unsigned int index = 0;

	if (!matrix)
		return *this;

	T *newmatrix = new T[size];


	while(xbl <= xtr)
	{
		int x, y;

		if (d == 1)
		{
			x = xtr;
			y = ytr;
		}
		else
		{
			x = xbl;
			y = ybl;
		}


		do
		{
			if (index >= size)
				throw "Matrix: Zigzag out of bounds";

			newmatrix[index++] = matrix[x+y*width];

			x -= d;
			y += d;
		} 
		while( x >= xbl && y <= ybl && x <= xtr && y >= ytr );

		d = -d;

		if (ybl == heightmin)
			xbl++;
		else
			ybl++;

		if (xtr == widthmin)
			ytr++;
		else
			xtr++;
	}

	delete[] matrix;

	matrix = newmatrix;

	return *this;
}


template <class T>
Matrix<T>& Matrix<T>::ZigzagInv()
{
	int xbl = 0, ybl = 0, xtr = 0, ytr = 0;
	int widthmin = width-1, heightmin = height-1;
	int d = -1;
	unsigned int size = height * width;
	unsigned int index = 0;

	if (!matrix)
		return *this;

	T *newmatrix = new T[size];


	while(xbl <= xtr)
	{
		int x, y;

		if (d == 1)
		{
			x = xtr;
			y = ytr;
		}
		else
		{
			x = xbl;
			y = ybl;
		}


		do
		{
			if (index >= size)
				throw "Matrix: ZigzagInv out of bounds";

			newmatrix[x+y*width] = matrix[index++];

			x -= d;
			y += d;
		} 
		while( x >= xbl && y <= ybl && x <= xtr && y >= ytr );

		d = -d;

		if (ybl == heightmin)
			xbl++;
		else
			ybl++;

		if (xtr == widthmin)
			ytr++;
		else
			xtr++;
	}

	delete[] matrix;

	matrix = newmatrix;

	return *this;
}

template <class T>
Matrix<T>& Matrix<T>::DivideElementsBy(const Matrix& rhs)
{
	unsigned int size = GetSize();

	if (width != rhs.width || height != rhs.height)
		throw "Matrix: DivideElementsBy invalid dimensions";

	for(unsigned int i=0; i<size; i++)
		matrix[i] /= rhs.matrix[i];

	return *this;
}

template <class T>
Matrix<T>& Matrix<T>::MultiplyElementsBy(const Matrix& rhs)
{
	unsigned int size = GetSize();

	if (width != rhs.width || height != rhs.height)
		throw "Matrix: MultiplyElementsBy invalid dimensions";


	for(unsigned int i=0; i<size; i++)
		matrix[i] *= rhs.matrix[i];

	return *this;
}


template <class T>
Matrix<T>& Matrix<T>::Transpose()
{
	T *newmatrix = new T[height * width];

	unsigned int newWidth = height;
	unsigned int newHeight = width;

	for(unsigned int y=0; y<newHeight; y++)
		for(unsigned int x=0; x<newWidth; x++)
		{
			newmatrix[x+y*newWidth] = matrix[y+x*width];
		}

	Clear();
	matrix = newmatrix;
	width = newWidth;
	height = newHeight;

	return *this;
}

template <class T>
Matrix<T> Matrix<T>::Transposed()
{
	Matrix result = *this;

	return result.Transpose();
}



template <class T>
void Matrix<T>::Assign(const Matrix<T>& rhs)
{
	if (this == &rhs) 
		return;

	Clear();

	unsigned int newWidth = rhs.GetWidth();
	unsigned int newHeight = rhs.GetHeight();
	unsigned int size = newWidth * newHeight;

	T *newmatrix = new T[size];

	for(unsigned int i=0; i<size; i++)
		newmatrix[i] = rhs.matrix[i];

	matrix = newmatrix;
	width = newWidth;
	height = newHeight;
}

template <class T>
void Matrix<T>::Add(const Matrix<T>& rhs)
{
	if (GetWidth() != rhs.GetWidth() || GetHeight() != rhs.GetHeight())
		throw "Matrix: Add invalid dimensions";
	
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
			matrix[x+y*width] += rhs.matrix[x+y*width];
}

template <class T>
void Matrix<T>::Substract(const Matrix<T>& rhs)
{
	if (GetWidth() != rhs.GetWidth() || GetHeight() != rhs.GetHeight())
		throw "Matrix: Substract invalid dimensions";
	
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
			matrix[x+y*width] -= rhs.matrix[x+y*width];
}

template <class T>
Matrix<T>& Matrix<T>::Scale(T scale)
{
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
			matrix[x+y*width] *= scale;

	return *this;
}

template <class T>
Matrix<T>& Matrix<T>::ScaleInv(T scale)
{
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
			matrix[x+y*width] /= scale;

	return *this;
}

template <class T>
void Matrix<T>::Floor()
{
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
		{
			T val = matrix[x+y*width];

			val = floor(val);
	
			matrix[x+y*width] = val;
		}
}

template <class T>
void Matrix<T>::Round()
{
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
		{
			T val = matrix[x+y*width];

			if (val < 0)
				val = -floor(-val + 0.5f);
			else
				val = floor(val + 0.5f);
	
			matrix[x+y*width] = val;
		}
}


template <class T>
void Matrix<T>::Clip(T minimum, T maximum)
{
	for(unsigned int y=0; y<height; y++)
		for(unsigned int x=0; x<width; x++)
		{
			T val = matrix[x+y*width];

			if (val < minimum)
				val = minimum;
			else if (val > maximum)
				val = maximum;

			matrix[x+y*width] = val;
		}
}



template <class T>
void Matrix<T>::ImportUnsignedChars(const unsigned char* data)
{
	unsigned int size = GetWidth() * GetHeight();

	for(unsigned int i=0; i<size; i++)
		matrix[i] = data[i];
}

template <class T>
void Matrix<T>::ImportSignedChars(const signed char* data)
{
	unsigned int size = GetWidth() * GetHeight();

	for(unsigned int i=0; i<size; i++)
		matrix[i] = data[i];
}

template <class T>
void Matrix<T>::ImportSignedInts(const signed int* data)
{
	unsigned int size = GetWidth() * GetHeight();

	for(unsigned int i=0; i<size; i++)
		matrix[i] = data[i];
}

template <class T>
void Matrix<T>::ExportUnsignedChars(unsigned char* data)
{
	unsigned int size = GetSize();

	for(unsigned int i=0; i<size; i++)
		data[i] = (unsigned char)matrix[i];
}

template <class T>
void Matrix<T>::ExportSignedChars(signed char* data)
{
	unsigned int size = GetSize();

	for(unsigned int i=0; i<size; i++)
		data[i] = (signed char)matrix[i];
}

template <class T>
void Matrix<T>::ExportSignedInts(signed int* data)
{
	unsigned int size = GetSize();

	for(unsigned int i=0; i<size; i++)
		data[i] = (signed int)matrix[i];
}

template <class T>
void Matrix<T>::Import(const T* data)
{
	unsigned int size = GetWidth() * GetHeight();

	for(unsigned int i=0; i<size; i++)
		matrix[i] = data[i];
}


template <class T>
void Matrix<T>::Multiply(const Matrix<T>& rhs)
{
	// Multiply this matrix with rhs

	// n x m * l x q
	// n x m * m x q = n x q

	


	unsigned int n = height;
	unsigned int m = width;
	unsigned int l = rhs.GetHeight();
	unsigned int q = rhs.GetWidth();

	if (m != l)
		throw "Matrix: Multiply invalid dimensions";

#ifndef SLOW
	// optimize 4x4 * 4x4 multiply
	if (n == 4 && m == 4 && q == 4)
	{
		T finalrow[4];

		for(unsigned int y=0; y<n; y++)
		{
			finalrow[0] = finalrow[1] = finalrow[2] = finalrow[3] = 0;

			for(unsigned int x=0; x<q; x++)
			{
				for(unsigned int k=0; k<m; k++)
				{
					finalrow[x] += GetElement(y, k) * rhs.GetElement(k, x);
				}
			}

			SetElement(y, 0, finalrow[0]);
			SetElement(y, 1, finalrow[1]);
			SetElement(y, 2, finalrow[2]);
			SetElement(y, 3, finalrow[3]);
		}
	}
	else
#endif
	{
		T *newmatrix = new T[n * q];
		
		for(unsigned int y=0; y<n; y++)
		{
			for(unsigned int x=0; x<q; x++)
			{
				T sum = 0;

				for(unsigned int k=0; k<m; k++)
				{
					sum += GetElement(y, k) * rhs.GetElement(k, x);
				}

				newmatrix[x + y*q] = sum;
			}
		}

		Clear();

		matrix = newmatrix;
		width = q;
		height = n;
	}
}




template <class T>
T& Matrix<T>::operator()(unsigned int y, unsigned int x)
{
	if(x >= width || y >= height)
		throw "Matrix: Operator() out of bounds";

	return matrix[x + y * width];
}



#endif // MATRIX_H